{
  "cells": [
    {
      "cell_type": "markdown",
      "id": "83ec2b32",
      "metadata": {},
      "source": [
        "# InvariantEKF User Guide\n",
        "\n",
        "This short guide summarizes the `InvariantEKF` API (and the related `LieGroupEKF`) and points to concrete examples in the repository.\n",
        "\n",
        "## What is the InvariantEKF?\n",
        "- A left-invariant EKF on a Lie group `G`.\n",
        "- State transition can be given by right multiplication (`predict(U, Q)`) or by a left–right pair (`predict(W, U, Q)`) that executes `X_{k+1} = W · X_k · U`."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "5381b579",
      "metadata": {},
      "source": [
        "## Python usage sketch"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "id": "76d8bece",
      "metadata": {},
      "outputs": [],
      "source": [
        "import numpy as np\n",
        "import gtsam\n",
        "\n",
        "# State type\n",
        "G = gtsam.Pose2\n",
        "\n",
        "# Initialize filter\n",
        "X0 = G.Identity()\n",
        "P0 = np.eye(3) * 0.1\n",
        "ekf = gtsam.InvariantEKFPose2(X0, P0)\n",
        "\n",
        "# Right-increment predict with discrete Q\n",
        "U = G(1.0, 0.0, 0.1)\n",
        "Qd = np.eye(3) * 0.01\n",
        "ekf.predict(U, Qd)\n",
        "\n",
        "# Left–right predict: X <- W * X * U\n",
        "W = G(0.1, 0.0, 0.0)\n",
        "ekf.predict(W, U, Qd)\n",
        "\n",
        "# In python, update using ManifoldEKF updateWithVector\n",
        "X = ekf.state()\n",
        "prediction = np.array([X.x(), X.y()])\n",
        "H = np.array([[1, 0, 0], [0, 1, 0]])\n",
        "z = np.array([1.0, 0.0])\n",
        "R = np.eye(2) * 0.01\n",
        "ekf.updateWithVector(prediction, H, z, R)"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "796ed3be",
      "metadata": {},
      "source": [
        "## Usage sketch (C++)\n",
        "```cpp\n",
        "using namespace gtsam;\n",
        "using G = Pose2;\n",
        "Matrix3 P0 = I_3x3 * 0.1;\n",
        "InvariantEKF<G> ekf(G(), P0);\n",
        "\n",
        "G U(1.0, 0.0, 0.1);\n",
        "Matrix3 Qd = I_3x3 * 0.01;\n",
        "ekf.predict(U, Qd);            // right increment\n",
        "G W(0.1, 0.0, 0.0);\n",
        "ekf.predict(W, U, Qd);         // left–right increment\n",
        "\n",
        "auto h = [](const G& X, OptionalJacobian<2,3> H) {\n",
        "    if (H) *H << 1, 0, 0, 0, 1, 0;\n",
        "    return Vector2(X.x(), X.y());\n",
        "};\n",
        "Vector2 z; z << 1.0, 0.0;\n",
        "Matrix2 R = Matrix2::Identity() * 0.01;\n",
        "ekf.update(h, z, R);\n",
        "```"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "55098c65",
      "metadata": {},
      "source": [
        "## Notes\n",
        "- `predict(W, U, Q)` treats `Q` as **discrete** (no `dt`).\n",
        "- `predict(u, dt, Q)` uses continuous-time `Q` and scales by `dt` internally.\n",
        "- IMU-specific EKFs (`NavStateImuEKF`, `Gal3ImuEKF`) reuse these interfaces; see the notebooks above for end-to-end flows."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "7a292821",
      "metadata": {},
      "source": [
        "## Where it is used today\n",
        "- C++ tests: [gtsam/navigation/tests/testInvariantEKF.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/tests/testInvariantEKF.cpp) exercises `predict(U, Q)` and the `dt` overload.\n",
        "- C++ examples: [examples/IEKF_NavstateExample.cpp](https://github.com/borglab/gtsam/blob/develop/examples/IEKF_NavstateExample.cpp) (NavState), [examples/GEKF_Rot3Example.cpp](https://github.com/borglab/gtsam/blob/develop/examples/GEKF_Rot3Example.cpp) (LieGroupEKF on SO(3)).\n",
        "- Docs: [gtsam/navigation/doc/InvariantEKF.md](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/doc/InvariantEKF.md) (math/background).\n",
        "- Python notebooks with IMU-specialized EKFs built on the same interface:\n",
        "  - [python/gtsam/examples/NavStateImuASVExample.ipynb](https://github.com/borglab/gtsam/blob/develop/python/gtsam/examples/NavStateImuASVExample.ipynb)\n",
        "  - [python/gtsam/examples/Gal3ImuExample.ipynb](https://github.com/borglab/gtsam/blob/develop/python/gtsam/examples/Gal3ImuExample.ipynb)\n",
        "  - [python/gtsam/examples/Gal3ImuASVExample.ipynb](https://github.com/borglab/gtsam/blob/develop/python/gtsam/examples/Gal3ImuASVExample.ipynb)\n",
        "\n",
        "If you add new examples that use `predict(W, U, Q)`, please link them here for discoverability."
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}
